#include "MIXER.h"

//舵面控制权重      横滚权重  俯仰权重  推进权重  偏航权重  直接力控制权重  舵面来流指向权重
#ifdef pwmPin1
float mixer1[6] = {-0.5,    0.2,      0,        0,      0.2,          -1};//鸭翼左
#endif
#ifdef pwmPin2
float mixer2[6] = {-0.5,    -0.2,     0,        0,      -0.2,         1};//鸭翼右
#endif
#ifdef pwmPin3
float mixer3[6] = {0,       0,        1,        0,      0,            0};//电机
#endif
#ifdef pwmPin4
float mixer4[6] = {0.3,     -0.7,     0,        0,      0.3,          0};//副翼左
#endif
#ifdef pwmPin5
float mixer5[6] = {-0.3,    -0.7,     0,        0,      0.3,          0};//副翼右
#endif
#ifdef pwmPin6
float mixer6[6] = {0,       0,        0,        0.5,    0,            0};//偏航1
#endif
#ifdef pwmPin7
float mixer7[6] = {0,       0,        0,        0.5,    0,            0};//偏航2
#endif
#ifdef pwmPin8
float mixer8[6] = {0,       0,        0,        0,      0,            0};//暂时闲置
#endif
#ifdef pwmPin9
float mixer9[6] = {0,       0,        0,        0,      0,            0};//暂时闲置
#endif
#ifdef pwmPin10
float mixer10[6] = {0,      0,        0,        0,      0,            0};//暂时闲置
#endif


//各舵机中立点设定，总共10个舵机，要改中立点直接改等于号后的数值就可以了
#ifdef pwmPin1
  #define trimCenter1 1500
#endif
#ifdef pwmPin2
  #define trimCenter2 1500
#endif
#ifdef pwmPin3
  #define trimCenter3 1500
#endif
#ifdef pwmPin4
  #define trimCenter4 1500
#endif
#ifdef pwmPin5
  #define trimCenter5 1500
#endif
#ifdef pwmPin6
  #define trimCenter6 1500
#endif
#ifdef pwmPin7
  #define trimCenter7 1500
#endif
#ifdef pwmPin8
  #define trimCenter8 1500
#endif
#ifdef pwmPin9
  #define trimCenter9 1500
#endif
#ifdef pwmPin10
  #define trimCenter10 1500
#endif

extern int RCPWM[chNum];//RCPWM的数据数组，所有飞控的输出数据直接放这个数组就可以了
extern float ctrl[6];//控制数据，横滚、俯仰、油门、偏航、直接力、AOA，归一法

void MIXER()
{//原理上讲就是矩阵相乘
  #ifdef pwmPin1
  RCPWM[0] = int((mixer1[0]*ctrl[0] + mixer1[1]*ctrl[1] + mixer1[2]*ctrl[2]
      + mixer1[3]*ctrl[3] + mixer1[4]*ctrl[4] + mixer1[5]*ctrl[5]) * 512) + trimCenter1;
  #endif
  #ifdef pwmPin2
  RCPWM[1] = int((mixer2[0]*ctrl[0] + mixer2[1]*ctrl[1] + mixer2[2]*ctrl[2]
      + mixer2[3]*ctrl[3] + mixer2[4]*ctrl[4] + mixer2[5]*ctrl[5]) * 512) + trimCenter2;
  #endif
  #ifdef pwmPin3
  RCPWM[2] = int((mixer3[0]*ctrl[0] + mixer3[1]*ctrl[1] + mixer3[2]*ctrl[2]
      + mixer3[3]*ctrl[3] + mixer3[4]*ctrl[4] + mixer3[5]*ctrl[5]) * 512) + trimCenter3;
  #endif
  #ifdef pwmPin4
  RCPWM[3] = int((mixer4[0]*ctrl[0] + mixer4[1]*ctrl[1] + mixer4[2]*ctrl[2]
      + mixer4[3]*ctrl[3] + mixer4[4]*ctrl[4] + mixer4[5]*ctrl[5]) * 512) + trimCenter4;
  #endif
  #ifdef pwmPin5
  RCPWM[4] = int((mixer5[0]*ctrl[0] + mixer5[1]*ctrl[1] + mixer5[2]*ctrl[2]
      + mixer5[3]*ctrl[3] + mixer5[4]*ctrl[4] + mixer5[5]*ctrl[5]) * 512) + trimCenter5;
  #endif
  #ifdef pwmPin6
  RCPWM[5] = int((mixer6[0]*ctrl[0] + mixer6[1]*ctrl[1] + mixer6[2]*ctrl[2]
      + mixer6[3]*ctrl[3] + mixer6[4]*ctrl[4] + mixer6[5]*ctrl[5]) * 512) + trimCenter6;
  #endif
  #ifdef pwmPin7
  RCPWM[6] = int((mixer7[0]*ctrl[0] + mixer7[1]*ctrl[1] + mixer7[2]*ctrl[2]
      + mixer7[3]*ctrl[3] + mixer7[4]*ctrl[4] + mixer7[5]*ctrl[5]) * 512) + trimCenter7;
  #endif
  #ifdef pwmPin8
  RCPWM[7] = int((mixer8[0]*ctrl[0] + mixer8[1]*ctrl[1] + mixer8[2]*ctrl[2]
      + mixer8[3]*ctrl[3] + mixer8[4]*ctrl[4] + mixer8[5]*ctrl[5]) * 512) + trimCenter8;
  #endif
  #ifdef pwmPin9
  RCPWM[8] = int((mixer9[0]*ctrl[0] + mixer9[1]*ctrl[1] + mixer9[2]*ctrl[2]
      + mixer9[3]*ctrl[3] + mixer9[4]*ctrl[4] + mixer9[5]*ctrl[5]) * 512) + trimCenter9;
  #endif
  #ifdef pwmPin10
  RCPWM[9] = int((mixer10[0]*ctrl[0] + mixer10[1]*ctrl[1] + mixer10[2]*ctrl[2]
      + mixer10[3]*ctrl[3] + mixer10[4]*ctrl[4] + mixer10[5]*ctrl[5]) * 512) + trimCenter10;
  #endif
}
